import ctypes
import time
import threading
# 依赖 Motor_ctl.py，需确保该文件包含以下导入项的定义
from Motor_ctl import (
    canDLL, TYPE_CAN, STATUS_OK, INVALID_DEVICE_HANDLE, INVALID_CHANNEL_HANDLE,
    ZCAN_Transmit_Data, ZCAN_Receive_Data, ZCAN_CAN_FRAME,
    CANMessageSequence, Motor_CTL, Motor_CTL_Pos, Motor_CTL_Ser,
    init_can_device, close_can_device  # 新增CAN设备初始化/关闭函数
)

class MoCTL_Cia(CANMessageSequence):
    """
    符合 CiA402 协议的电机控制类（适配 ZLAC8015D 驱动器）
    整合位置/速度/转矩三种控制模式，严格遵循文档3.1-3.7节状态机与控制规范
    """
    # -------------------------- CiA402 协议核心常量（适配文档3.1-3.5节） --------------------------
    # 状态机常量（复用 CANMessageSequence，符合文档3.1节状态机定义）
    STATE_NOT_READY_TO_SWITCH_ON = CANMessageSequence.STATE_NOT_READY_TO_SWITCH_ON
    STATE_SWITCH_ON_DISABLED = CANMessageSequence.STATE_SWITCH_ON_DISABLED
    STATE_READY_TO_SWITCH_ON = CANMessageSequence.STATE_READY_TO_SWITCH_ON
    STATE_SWITCHED_ON = CANMessageSequence.STATE_SWITCHED_ON
    STATE_OPERATION_ENABLED = CANMessageSequence.STATE_OPERATION_ENABLED
    STATE_QUICK_STOP_ACTIVE = CANMessageSequence.STATE_QUICK_STOP_ACTIVE
    STATE_FAULT_REACTION_ACTIVE = CANMessageSequence.STATE_FAULT_REACTION_ACTIVE
    STATE_FAULT = CANMessageSequence.STATE_FAULT

    # 控制字命令（符合文档3.2节控制字定义，如0x06=Shutdown、0x0F=Enable Operation）
    CONTROL_SHUTDOWN = CANMessageSequence.CONTROL_SHUTDOWN          
    CONTROL_SWITCH_ON = CANMessageSequence.CONTROL_SWITCH_ON        
    CONTROL_ENABLE_OPERATION = CANMessageSequence.CONTROL_ENABLE_OPERATION  
    CONTROL_DISABLE_VOLTAGE = CANMessageSequence.CONTROL_DISABLE_VOLTAGE    
    CONTROL_QUICK_STOP = CANMessageSequence.CONTROL_QUICK_STOP      
    CONTROL_DISABLE_OPERATION = CANMessageSequence.CONTROL_DISABLE_OPERATION  
    CONTROL_FAULT_RESET = CANMessageSequence.CONTROL_FAULT_RESET    

    # 操作模式（符合文档3.3-3.5节，1=位置/3=速度/4=转矩）
    MODE_POSITION = CANMessageSequence.MODE_POSITION                
    MODE_VELOCITY = CANMessageSequence.MODE_VELOCITY                
    MODE_TORQUE = CANMessageSequence.MODE_TORQUE                    

    # 控制方式（同步/异步，复用 Motor_CTL，符合文档3.4.2/3.5.2节同步控制逻辑）
    SYNC_CONTROL = Motor_CTL.SYNC_CONTROL        # 0：双电机同参数控制
    ASYNC_CONTROL = Motor_CTL.ASYNC_CONTROL      # 1：双电机独立参数控制

    # 对象字典索引（符合文档4章，如0x6040=控制字、0x603F=故障码）
    OD_CONTROL_WORD = CANMessageSequence.OD_CONTROL_WORD            
    OD_STATUS_WORD = CANMessageSequence.OD_STATUS_WORD              
    OD_MODES_OF_OPERATION = CANMessageSequence.OD_MODES_OF_OPERATION  
    OD_FAULT_CODE = 0x603F  # 文档3.7节故障码索引

    def __init__(self, channel_handle, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化 CiA402 电机控制器（适配文档2.1节SDO客户端/服务器ID定义）
        Args:
            channel_handle: CAN通道句柄（由 Motor_ctl.init_can_device() 生成）
            send_id: SDO客户端发送ID（默认0x601，文档2.1节COB-ID范围0x601-0x67F）
            response_id: SDO服务器响应ID（默认0x581，文档2.1节COB-ID范围0x581-0x5FF）
            response_timeout: 响应超时时间（秒，默认5s）
        """
        super().__init__(channel_handle, send_id, response_id, response_timeout)

        # 初始化 Motor_ctl.py 中的核心控制组件
        self.vel_ctl = Motor_CTL(  # 速度控制器（适配文档3.4节速度模式）
            channel_handle=channel_handle,
            send_id=send_id,
            response_id=response_id,
            response_timeout=response_timeout
        )
        self.pos_ctl = Motor_CTL_Pos(  # 位置控制器（适配文档3.3节位置模式）
            channel_handle=channel_handle,
            send_id=send_id,
            response_id=response_id,
            response_timeout=response_timeout
        )
        self.torque_ctl = Motor_CTL_Ser(  # 转矩控制器（适配文档3.5节转矩模式）
            channel_handle=channel_handle,
            send_id=send_id,
            response_id=response_id,
            response_timeout=response_timeout
        )

        # 运行状态变量（初始状态符合文档3.1节上电默认态）
        self.current_mode = None  # 当前操作模式（None/位置/速度/转矩）
        self.current_control = None  # 当前控制方式（None/同步/异步）
        self.current_state = self.STATE_SWITCH_ON_DISABLED  # 上电默认态
        self.last_fault_code = 0  # 最后一次故障码（文档3.7节）
        
        # 位置监控相关属性
        self.target_positions = None
        self.position_monitoring = False
        self.position_monitor_thread = None
        
        # 速度监控相关属性
        self.velocity_monitoring = False
        self.velocity_monitor_thread = None
        self.velocity_monitor_timeout = 5000

    # -------------------------- CiA402 状态机管理 --------------------------
    def read_status_word(self):
        """
        读取状态字（0x6041）并解析状态（文档3.2节状态字bit0-bit6定义）
        Returns:
            int: 状态字原始值（32位，低16位左电机/高16位右电机，文档3.2节）
            str: 解析后的状态（STATE_* 常量）
        """
        result = super().read_status_word()
        
        # 处理返回值，兼容新旧两种格式
        if result is None:
            print("❌ 读取状态字（0x6041）失败")
            print("⚠️ 请检查：")
            print("  1. CAN总线物理连接（线缆、终端电阻120Ω）")
            print("  2. 驱动器电源是否开启")
            print("  3. CAN波特率设置（默认1Mbps）")
            print("  4. 驱动器节点地址配置（默认0x01）")
            print("  5. 驱动器是否处于故障状态")
            return None, self.current_state
        
        # 判断返回值是否为元组（兼容修改后的格式）
        if isinstance(result, tuple):
            status_word, _ = result  # 只取第一个元素作为状态字
            if status_word is None:
                print("❌ 状态字值为None")
                return None, self.current_state
        else:
            status_word = result  # 保持向后兼容
        
        # 解析状态字bit0-bit6（文档3.2节状态字定义）
        try:
            bit0 = (status_word >> 0) & 0x01
            bit1 = (status_word >> 1) & 0x01
            bit2 = (status_word >> 2) & 0x01
            bit3 = (status_word >> 3) & 0x01
            bit5 = (status_word >> 5) & 0x01
            bit6 = (status_word >> 6) & 0x01
        except TypeError as e:
            print(f"❌ 状态字解析错误: {e}")
            print(f"   状态字值类型: {type(status_word)}, 值: {status_word}")
            return None, self.current_state

        if bit6 == 0 and bit5 == 0 and bit3 == 0 and bit2 == 0 and bit1 == 0 and bit0 == 0:
            # 所有状态位为0，可能是驱动器未初始化或配置问题
            self.current_state = self.STATE_NOT_READY_TO_SWITCH_ON
            print("⚠️  状态字所有位为0，驱动器可能未正确初始化或配置")
        elif bit6 == 1 and bit5 == 0 and bit3 == 0 and bit2 == 0 and bit1 == 0 and bit0 == 0:
            self.current_state = self.STATE_SWITCH_ON_DISABLED
        elif bit6 == 0 and bit5 == 1 and bit3 == 0 and bit2 == 0 and bit1 == 0 and bit0 == 1:
            self.current_state = self.STATE_READY_TO_SWITCH_ON
        elif bit6 == 0 and bit5 == 1 and bit3 == 0 and bit2 == 0 and bit1 == 1 and bit0 == 1:
            self.current_state = self.STATE_SWITCHED_ON
        elif bit6 == 0 and bit5 == 1 and bit3 == 0 and bit2 == 1 and bit1 == 1 and bit0 == 1:
            self.current_state = self.STATE_OPERATION_ENABLED
        elif bit6 == 0 and bit5 == 0 and bit3 == 0 and bit2 == 1 and bit1 == 1 and bit0 == 1:
            self.current_state = self.STATE_QUICK_STOP_ACTIVE
        elif bit6 == 0 and bit5 == 0 and bit3 == 1 and bit2 == 1 and bit1 == 1 and bit0 == 1:
            self.current_state = self.STATE_FAULT_REACTION_ACTIVE
        elif bit6 == 0 and bit5 == 0 and bit3 == 1 and bit2 == 0 and bit1 == 0 and bit0 == 0:
            self.current_state = self.STATE_FAULT
        else:
            self.current_state = "Unknown state"

        print(f"✅ 当前状态: {self.get_state_name(self.current_state)} | 状态字: 0x{status_word:08X}")
        return status_word, self.current_state

    def read_fault_code(self):
        """读取故障码（0x603F）并解析"""
        fault_code, _ = self._send_and_receive_sdo(0x603F, 0)
        if fault_code is not None:
            print(f"⚠️ 当前故障码: 0x{fault_code:04X}")
            # 根据文档3.7节解析故障码
            if fault_code == 0x0000:
                print("   无故障")
            elif fault_code == 0x2310:
                print("   过流故障")
            elif fault_code == 0x3210:
                print("   过压故障") 
            elif fault_code == 0x3220:
                print("   欠压故障")
            elif fault_code == 0x4310:
                print("   过温故障")
            else:
                print(f"   未知故障码，请参考文档3.7节")
        return fault_code

    def fault_reset(self):
        """
        故障复位（发送0x80控制字，文档3.7节故障清除流程）
        Returns:
            bool: 复位是否成功
        """
        _, current_state = self.read_status_word()
        if current_state != self.STATE_FAULT:
            print(f"⚠️ 当前状态非故障态（{self.get_state_name(current_state)}），无需复位")
            return True

        # 发送故障复位序列（文档3.7节故障清除命令）
        self.add_cia402_sequence("fault_reset")
        success = self.send_sequence(len(self.message_sequences) - 1)
        if success:
            print("✅ 故障复位成功，正在确认状态...")
            self.read_status_word()  # 复位后确认状态切换
        else:
            print("❌ 故障复位命令发送失败")
        return success

    # -------------------------- 控制模式切换 --------------------------
    def set_operation_mode(self, target_mode):
        """
        设置操作模式（位置/速度/转矩，文档3.3-3.5节），增强状态检测和重试机制
        Args:
            target_mode: 目标模式（MODE_POSITION/MODE_VELOCITY/MODE_TORQUE）
        Returns:
            bool: 模式切换是否成功
        """
        # 校验模式合法性（文档3.3-3.5节支持的模式）
        if target_mode not in [self.MODE_POSITION, self.MODE_VELOCITY, self.MODE_TORQUE]:
            print(f"❌ 无效模式 {target_mode}，仅支持：位置(1)/速度(3)/转矩(4)")
            return False

        # 获取当前状态
        status_word, current_state = self.read_status_word()
        
        # 如果无法获取状态，多次尝试
        retry_count = 0
        max_retries = 2
        while status_word is None and retry_count < max_retries:
            retry_count += 1
            print(f"⚠️  读取状态字失败，第{retry_count}次重试...")
            time.sleep(0.2)
            status_word, current_state = self.read_status_word()
        
        # 如果多次尝试后仍无法获取状态，使用备用逻辑
        if status_word is None:
            print("⚠️  无法读取状态字，尝试强制模式切换...")
        else:
            # 校验当前状态
            if current_state in [self.STATE_FAULT, self.STATE_FAULT_REACTION_ACTIVE]:
                print("❌ 故障态无法切换模式，请先执行 fault_reset()")
                return False
            
            # 对于急停状态，增加一个警告但允许继续尝试
            if current_state == self.STATE_QUICK_STOP_ACTIVE:
                print("⚠️  当前处于急停状态，尝试强制切换模式...")

        # 发送模式切换序列（SDO写0x6060索引，文档3.3-3.5节模式设置），增加重试机制
        max_mode_retries = 2
        mode_retry = 0
        success = False
        
        while not success and mode_retry < max_mode_retries:
            mode_retry += 1
            
            # 构建并发送模式切换序列
            self.add_cia402_sequence("set_mode", mode=target_mode)
            seq_index = len(self.message_sequences) - 1
            
            success = self.send_sequence(seq_index)
            
            if not success:
                print(f"⚠️  第{mode_retry}次模式切换失败，重试中...")
                time.sleep(0.3)
            else:
                break
        
        if success:
            self.current_mode = target_mode
            mode_name = self.get_mode_name(target_mode)
            print(f"✅ 操作模式切换为: {mode_name}（索引0x6060已更新）")
            
            # 初始化对应模式的控制器（复用 Motor_ctl 中的初始化逻辑）
            if target_mode == self.MODE_VELOCITY and self.vel_ctl is not None:
                # 确保控制模式设置正确
                if self.current_control is not None:
                    self.vel_ctl.set_control_mode(self.current_control)
                # 注意：初始化将在 set_control_method() 中进行，此处不再调用
            
            elif target_mode == self.MODE_POSITION and self.pos_ctl is not None:
                # 位置模式需要重新初始化控制器，确保从之前的状态中恢复
                # 注意：这里不调用_build_init_sequence()，因为set_position_mode会在之后调用
                # 设置默认控制方式为同步控制（位置模式只支持同步）
                self.current_control = self.SYNC_CONTROL
                print("✅ 位置模式控制方式设置为: 同步（默认）")
                
                # 位置模式下额外确保位置控制参数正确
                self.pos_ctl.initialize_motor()
            
            elif target_mode == self.MODE_TORQUE and self.torque_ctl is not None:
                if self.current_control is not None:
                    self.torque_ctl.set_control_mode(self.current_control)
                # 注意：初始化将在 set_control_method() 中进行，此处不再调用
            
            # 验证模式是否真的切换成功
            verify_success = self._verify_operation_mode(target_mode)
            if not verify_success:
                print("⚠️  模式切换验证失败，可能需要重新初始化")
        else:
            print(f"❌ 操作模式切换为 {target_mode} 失败（已尝试{max_mode_retries}次，检查SDO通信是否正常）")
        
        return success
    
    def _verify_operation_mode(self, expected_mode):
        """
        验证操作模式是否正确设置
        
        Args:
            expected_mode: 期望的模式
            
        Returns:
            bool: 验证是否成功
        """
        try:
            # 使用SDO读取0x6060索引来验证模式设置
            cmd = self._build_sdo_read(0x6060, 0x00)
            success = self._send_can_data(self.send_id, cmd)
            
            if not success:
                return False
            
            response_success, response_data = self._receive_can_data(self.response_id, 0.5)
            
            if not response_success or len(response_data) < 8:
                return False
            
            # 解析实际的模式值（小端格式）
            actual_mode = (response_data[5] << 8) | response_data[4]
            
            if actual_mode == expected_mode:
                print(f"✅ 模式验证成功: 实际模式={actual_mode}，期望模式={expected_mode}")
                return True
            else:
                print(f"❌ 模式验证失败: 实际模式={actual_mode}，期望模式={expected_mode}")
                return False
        except Exception as e:
            print(f"⚠️  模式验证过程发生异常: {e}")
            return True  # 验证失败不应影响主流程

    def set_control_method(self, control_method):
        """
        设置控制方式（同步/异步，仅速度/转矩模式支持，文档3.4.2/3.5.2节）
        Args:
            control_method: 控制方式（SYNC_CONTROL/ASYNC_CONTROL）
        Returns:
            bool: 控制方式设置是否成功
        """
        if control_method not in [self.SYNC_CONTROL, self.ASYNC_CONTROL]:
            print(f"❌ 无效控制方式 {control_method}，仅支持：同步(0)/异步(1)")
            return False

        self.current_control = control_method
        # 初始化对应模式的控制方式（复用 Motor_ctl 中的控制逻辑）
        if self.current_mode == self.MODE_VELOCITY:
            self.vel_ctl.set_control_mode(control_method)
            self.vel_ctl.initialize_motor()
            print(f"✅ 速度模式控制方式设置为: {'同步' if control_method == 0 else '异步'}")
        elif self.current_mode == self.MODE_TORQUE:
            self.torque_ctl.set_control_mode(control_method)
            self.torque_ctl.initialize_motor()
            print(f"✅ 转矩模式控制方式设置为: {'同步' if control_method == 0 else '异步'}")
        else:
            print(f"⚠️ 位置模式无需设置控制方式（默认同步控制，文档3.3节）")
        return True

    # -------------------------- 电机启动/停止流程 --------------------------
    def power_on_sequence(self, max_retry=5):
        """
        上电流程（文档3.1节状态机切换）
        增加重试机制和详细错误诊断
        """
        print("🔧 开始上电流程...")
        
        # 先检查当前状态
        _, current_state = self.read_status_word()
        if current_state is None:
            print("❌ 无法读取初始状态，终止上电流程")
            return False
        
        print(f"📊 初始状态: {self.get_state_name(current_state)}")
        
        # 如果当前是故障状态，先复位
        if current_state == self.STATE_FAULT:
            print("⚠️ 检测到故障状态，尝试故障复位...")
            if not self.fault_reset():
                print("❌ 故障复位失败，终止上电流程")
                return False
            time.sleep(0.5)
            _, current_state = self.read_status_word()
        
        # 上电流程重试机制
        for retry_count in range(max_retry):
            print(f"🔄 上电尝试 {retry_count + 1}/{max_retry}")
            
            # 发送控制字序列（按照CiA402标准流程）
            # 1. 发送Shutdown控制字（0x06）
            print("   → 发送Shutdown控制字 (0x06)...")
            self.add_cia402_sequence("shutdown")
            if not self.send_sequence(len(self.message_sequences) - 1):
                print("❌ 发送Shutdown控制字失败")
                continue
            time.sleep(0.1)
            
            # 2. 发送Switch On控制字（0x07）
            print("   → 发送Switch On控制字 (0x07)...")
            self.add_cia402_sequence("switch_on")
            if not self.send_sequence(len(self.message_sequences) - 1):
                print("❌ 发送Switch On控制字失败")
                continue
            time.sleep(0.1)
            
            # 3. 发送Enable Operation控制字（0x0F）
            print("   → 发送Enable Operation控制字 (0x0F)...")
            self.add_cia402_sequence("enable")
            if not self.send_sequence(len(self.message_sequences) - 1):
                print("❌ 发送Enable Operation控制字失败")
                continue
            time.sleep(0.1)
            
            # 确认最终状态（需为OPERATION_ENABLED，文档3.1节上电成功态）
            final_status_word, final_state = self.read_status_word()
            if final_state == self.STATE_OPERATION_ENABLED:
                print("🎉 上电流程完成！电机已进入操作使能态")
                return True
            else:
                print(f"⚠️ 上电未完成，当前状态: {self.get_state_name(final_state)}")
                # 修复：只有当final_status_word不为None时才进行格式化
                if final_status_word is not None:
                    print(f"   状态字: 0x{final_status_word:08X}")
                else:
                    print("   状态字: 读取失败（None）")
                
                # 诊断状态字具体问题
                if final_status_word is not None:
                    self._diagnose_status_word(final_status_word)
            
        print("❌ 上电流程失败，已达到最大重试次数")
        return False
    
    def _diagnose_status_word(self, status_word):
        """诊断状态字具体问题"""
        bit0 = (status_word >> 0) & 0x01  # Ready to switch on
        bit1 = (status_word >> 1) & 0x01  # Switched on  
        bit2 = (status_word >> 2) & 0x01  # Operation enabled
        bit3 = (status_word >> 3) & 0x01  # Fault
        bit5 = (status_word >> 5) & 0x01  # Quick stop
        bit6 = (status_word >> 6) & 0x01  # Switch on disabled
        
        print("🔍 状态字诊断:")
        print(f"   Bit0 (Ready to switch on): {bit0}")
        print(f"   Bit1 (Switched on): {bit1}") 
        print(f"   Bit2 (Operation enabled): {bit2}")
        print(f"   Bit3 (Fault): {bit3}")
        print(f"   Bit5 (Quick stop): {bit5}")
        print(f"   Bit6 (Switch on disabled): {bit6}")
        
        if bit3 == 1:
            print("⚠️ 驱动器存在故障，请先清除故障")
            self.read_fault_code()

    def power_off_sequence(self):
        """
        下电流程：OPERATION_ENABLED → SWITCHED_ON → READY_TO_SWITCH_ON → SWITCH_ON_DISABLED（文档3.1节）
        Returns:
            bool: 下电流程是否成功
        """
        # 步骤1：Disable Operation（切换至SWITCHED_ON，控制字0x07，文档3.2节）
        self.add_cia402_sequence("disable")
        if not self.send_sequence(len(self.message_sequences) - 1):
            print("❌ 下电步骤1：发送Disable Operation命令失败")
            return False
        time.sleep(0.1)

        # 步骤2：Shutdown（切换至READY_TO_SWITCH_ON，控制字0x06，文档3.2节）
        self.add_cia402_sequence("shutdown")
        if not self.send_sequence(len(self.message_sequences) - 1):
            print("❌ 下电步骤2：发送Shutdown命令失败")
            return False
        time.sleep(0.1)

        # 步骤3：Disable Voltage（切换至SWITCH_ON_DISABLED，控制字0x00，文档3.2节）
        self.add_cia402_sequence("init")  # init序列包含禁用电压命令
        if not self.send_sequence(len(self.message_sequences) - 1):
            print("❌ 下电步骤3：发送Disable Voltage命令失败")
            return False
        time.sleep(0.1)

        # 确认最终状态（需为SWITCH_ON_DISABLED，文档3.1节下电成功态）
        _, final_state = self.read_status_word()
        if final_state != self.STATE_SWITCH_ON_DISABLED:
            print(f"❌ 下电流程未完成，最终状态为 {self.get_state_name(final_state)}")
            return False
        print("🎉 下电流程完成！电机已进入上电禁用态（SWITCH_ON_DISABLED）")
        return True

    def quick_stop(self):
        """
        快速停止（切换至QUICK_STOP_ACTIVE，文档3.6节急停流程）
        Returns:
            bool: 快速停止是否成功
        """
        self.add_cia402_sequence("quick_stop")
        success = self.send_sequence(len(self.message_sequences) - 1)
        if success:
            print("✅ 快速停止命令（控制字0x02）发送成功")
            self.read_status_word()  # 确认切换至QUICK_STOP_ACTIVE
        else:
            print("❌ 快速停止命令发送失败")
        return success

    # -------------------------- 位置模式控制（适配文档3.3节） --------------------------
    def set_position(self, position_mode, left_pos, right_pos, auto_reactivate=True):
        """
        设置位置并激活位置控制（合并原set_position和_reactivate_position_control功能）
        
        Args:
            position_mode: 位置模式（Motor_CTL_Pos.RELATIVE_POSITION/ABSOLUTE_POSITION）
            left_pos: 左电机目标位置（counts，文档3.3.1节范围：相对±0x7FFFFFFF）
            right_pos: 右电机目标位置（counts，文档3.3.1节范围：绝对±0x3FFFFFFF）
            auto_reactivate: 是否自动重新激活位置控制，默认True
            
        Returns:
            bool: 位置设置和激活是否成功
        """
        if self.current_mode != self.MODE_POSITION:
            print(f"❌ 当前模式为 {self.get_mode_name(self.current_mode)}，需先调用 set_position_mode() 切换至位置模式")
            return False

        # 调用 Motor_CTL_Pos 的位置设置方法
        success = self.pos_ctl.set_position(
            position_mode=position_mode,
            left_position=left_pos,
            right_position=right_pos
        )
        
        if not success:
            print("❌ 位置设置失败")
            return False
            
        print(f"✅ 位置设置成功：左电机{left_pos}counts / 右电机{right_pos}counts（{('相对','绝对')[position_mode]}模式）")
        
        # 如果需要自动重新激活位置控制
        if auto_reactivate:
            print("🔄 激活位置控制...")
            
            try:
                # 重置控制字
                reset_cmd = self._build_sdo_write(
                    index=0x6040,  # 控制字索引
                    subindex=0x00,
                    data=0x0F,     # 重置控制字值
                    data_size=2
                )
                reset_success = self.pos_ctl._send_can_data(self.send_id, reset_cmd)
                if not reset_success:
                    print("❌ 重置控制字失败")
                    return False
                
                time.sleep(0.1)
                
                # 发送激活命令
                control_word = 0x0F if position_mode == 0 else 0x1F  # 相对位置0x0F，绝对位置0x1F
                activate_cmd = self._build_sdo_write(
                    index=0x6040,
                    subindex=0x00,
                    data=control_word,
                    data_size=2
                )
                activate_success = self.pos_ctl._send_can_data(self.send_id, activate_cmd)
                if not activate_success:
                    print("❌ 发送激活命令失败")
                    return False
                
                time.sleep(0.1)
                
                # 验证位置控制是否已激活
                status_word, _ = self.read_status_word()
                if status_word is not None:
                    bit14 = (status_word >> 14) & 0x01  # 位置控制激活位
                    print(f"   状态字: 0x{status_word:04X}, bit14(位置控制): {bit14}")
                    
                    if bit14 == 1:
                        print("✅ 位置控制激活成功")
                        return True
                    else:
                        print("⚠️  位置控制未激活，尝试备用激活方案...")
                        # 备用激活方案
                        control_word = 0x0F if position_mode == 0 else 0x1F
                        cmd = self._build_sdo_write(
                            index=0x6040,
                            subindex=0x00,
                            data=control_word,
                            data_size=2
                        )
                        
                        alt_success = self.pos_ctl._send_can_data(self.send_id, cmd)
                        if alt_success:
                            time.sleep(0.1)
                            status_word, _ = self.read_status_word()
                            if status_word is not None and (status_word >> 14) & 0x01 == 1:
                                print("✅ 备用激活方案成功")
                                return True
                        
                        print("❌ 位置控制激活失败")
                        return False
                else:
                    print("❌ 无法读取状态字验证激活状态")
                    return False
                    
            except Exception as e:
                print(f"❌ 激活位置控制时发生错误: {str(e)}")
                return False
        
        return True

    def set_position_mode(self, max_speed=120, left_accel=100, right_accel=100, left_decel=100, right_decel=100):
        """
        切换到位置模式并设置参数（合并原set_position_mode和set_position_params功能）
        
        Args:
            max_speed: 最大速度（r/min），限制在1-1000范围内，默认120r/min
            left_accel: 左电机加速时间（ms，0-32767，默认100ms）
            right_accel: 右电机加速时间（ms，0-32767，默认100ms）
            left_decel: 左电机减速时间（ms，0-32767，默认100ms）
            right_decel: 右电机减速时间（ms，0-32767，默认100ms）
            
        Returns:
            bool: 模式切换和参数设置是否成功
        """
        print("⚙️  切换到位置模式并设置参数...")
        
        try:
            # 检查是否处于急停状态
            status_word, current_state = self.read_status_word()
            
            # 多次尝试解除急停状态
            max_estop_retries = 3
            estop_retry_count = 0
            estop_cleared = False
            
            while status_word is not None and ((status_word >> 5) & 0x01) == 0 and estop_retry_count < max_estop_retries:
                estop_retry_count += 1
                print(f"⚠️  检测到急停状态: 0x{status_word:04X}（bit5=0），第{estop_retry_count}次尝试解除急停...")
                
                # 解除急停
                if not self._clear_quick_stop_state():
                    print(f"❌ 第{estop_retry_count}次解除急停失败，等待重试...")
                    time.sleep(0.3)
                else:
                    print("✅ 急停状态已成功解除")
                    estop_cleared = True
                    break
                
                # 重新读取状态
                status_word, current_state = self.read_status_word()
            
            # 如果急停已解除，等待状态确认
            if estop_cleared:
                print("⏳ 等待急停状态确认...")
                confirmation_timeout = 5  # 5秒确认超时
                start_time = time.time()
                
                while time.time() - start_time < confirmation_timeout:
                    status_word, current_state = self.read_status_word()
                    if status_word is not None and ((status_word >> 5) & 0x01) == 1:
                        print("✅ 急停状态确认完成，bit5=1（急停已解除）")
                        break
                    time.sleep(0.1)
                else:
                    print("⚠️  急停状态确认超时，但继续执行模式切换")
            
            # 最终检查急停状态
            if status_word is not None and ((status_word >> 5) & 0x01) == 0:
                print("❌ 多次尝试解除急停失败，模式切换中止")
                return False
            
            # 设置操作模式为位置模式（增加重试机制）
            mode_success = False
            for mode_retry in range(2):
                if self.set_operation_mode(self.MODE_POSITION):
                    mode_success = True
                    print("✅ 成功切换到位置模式")
                    break
                print(f"⚠️  位置模式切换失败，第{mode_retry+1}次重试...")
                time.sleep(0.2)
            
            if not mode_success:
                print("❌ 切换到位置模式失败")
                return False
            
            # 设置位置模式参数
            print("⚙️  设置位置模式参数...")
            params_seq = [
                self._build_sdo_write(0x6083, 0x01, left_accel, 4),  # 左电机加速时间
                self._build_sdo_write(0x6083, 0x02, right_accel, 4), # 右电机加速时间
                self._build_sdo_write(0x6084, 0x01, left_decel, 4),  # 左电机减速时间
                self._build_sdo_write(0x6084, 0x02, right_decel, 4), # 右电机减速时间
                self._build_sdo_write(0x6081, 0x01, max_speed, 4),   # 左电机最大速度
                self._build_sdo_write(0x6081, 0x02, max_speed, 4)    # 右电机最大速度
            ]
            self.add_sequence("位置模式参数设置", params_seq)
            params_success = self.send_sequence(len(self.message_sequences) - 1)
            
            if not params_success:
                print("❌ 位置模式参数设置失败")
                return False
            
            # 初始化位置控制器
            print("⚙️  初始化位置控制器...")
            self.pos_ctl.set_max_velocity(max_speed)
            
            init_success = False
            for param_retry in range(2):
                if self.pos_ctl.initialize_motor():
                    init_success = True
                    print("✅ 位置控制器初始化完成")
                    break
                print(f"⚠️  位置控制器初始化失败，第{param_retry+1}次重试...")
                time.sleep(0.2)
            
            if not init_success:
                print("❌ 位置控制器初始化失败")
                return False
            
            print(f"✅ 位置模式设置成功：最大速度{max_speed}r/min，加速{left_accel}ms/减速{left_decel}ms")
            return True
            
        except Exception as e:
            print(f"❌ 设置位置模式时发生错误: {e}")
            return False

    # -------------------------- 速度模式控制（适配文档3.4节） --------------------------
    def set_velocity(self, speed, run_time_ms=None, left_speed=None, right_speed=None):
        """
        设置速度（同步/异步，文档3.4.2节例程）
        Args:
            speed: 同步模式速度（r/min，-1000~1000）；异步模式默认速度
            run_time_ms: 运行时间（ms，超时自动停止，默认None=持续运行）
            left_speed: 异步模式左电机速度（r/min，-1000~1000，需设置current_control=1）
            right_speed: 异步模式右电机速度（r/min，-1000~1000，需设置current_control=1）
        Returns:
            bool: 速度设置是否成功
        """
        if self.current_mode != self.MODE_VELOCITY:
            print(f"❌ 当前模式为 {self.get_mode_name(self.current_mode)}，需先调用 set_operation_mode(3) 切换至速度模式")
            return False
        if self.current_control is None:
            print("❌ 未设置控制方式，需先调用 set_control_method(SYNC_CONTROL/ASYNC_CONTROL)")
            return False

        # 启动速度监控（无论是否有运行时间参数都需要监控速度设置是否生效）
        if run_time_ms is not None:
            # 有运行时间参数：启动后台速度监控线程，监控超时时间设置为运行时间+2秒的缓冲
            print("🔄 检测到运行时间参数，启动后台速度监控...")
            monitor_timeout = run_time_ms + 2000
            self.monitor_velocity_status(timeout_ms=monitor_timeout, background=True)
        else:
            # 持续运行模式：启动后台速度监控线程，监控超时时间设置为10秒
            print("🔄 持续运行模式，启动后台速度监控...")
            self.monitor_velocity_status(timeout_ms=10000, background=True)

        # 调用 Motor_CTL 的速度设置方法（符合文档3.4.2节同步/异步控制逻辑）
        success = self.vel_ctl.set_speed(
            speed=speed,
            run_time_ms=run_time_ms,
            left_speed=left_speed,
            right_speed=right_speed
        )
        if success:
            mode_desc = f"同步速度{speed}r/min" if self.current_control == 0 else f"左{left_speed}r/min/右{right_speed}r/min"
            print(f"✅ 速度设置成功：{mode_desc}（运行时间：{run_time_ms}ms）")
        return success

    def set_velocity_params(self, left_accel=100, right_accel=100, left_decel=100, right_decel=100):
        """
        设置速度模式参数（S形加减速时间，文档3.4.1节参数定义）
        Args:
            left_accel: 左电机加速时间（ms，0-32767，默认100ms）
            right_accel: 右电机加速时间（ms，0-32767，默认100ms）
            left_decel: 左电机减速时间（ms，0-32767，默认100ms）
            right_decel: 右电机减速时间（ms，0-32767，默认100ms）
        Returns:
            bool: 参数设置是否成功
        """
        # 构建SDO参数设置序列（文档3.4.2节例程：0x6083=加速时间、0x6084=减速时间）
        params_seq = [
            self._build_sdo_write(0x6083, 0x01, left_accel, 4),  # 左电机加速时间
            self._build_sdo_write(0x6083, 0x02, right_accel, 4), # 右电机加速时间
            self._build_sdo_write(0x6084, 0x01, left_decel, 4),  # 左电机减速时间
            self._build_sdo_write(0x6084, 0x02, right_decel, 4)  # 右电机减速时间
        ]
        self.add_sequence("速度模式参数设置", params_seq)
        success = self.send_sequence(len(self.message_sequences) - 1)
        if success:
            print(f"✅ 速度模式参数设置成功：加速{left_accel}ms/减速{left_decel}ms（左右电机一致）")
        else:
            print("❌ 速度模式参数设置失败（检查SDO写命令索引/子索引）")
        return success

    # -------------------------- 转矩模式控制（适配文档3.5节） --------------------------
    def set_torque(self, torque, left_torque=None, right_torque=None):
        """
        设置转矩（同步/异步，文档3.5.2节例程）
        Args:
            torque: 同步模式转矩（mA，-30000~30000）；异步模式默认转矩
            left_torque: 异步模式左电机转矩（mA，-30000~30000，需设置current_control=1）
            right_torque: 异步模式右电机转矩（mA，-30000~30000，需设置current_control=1）
        Returns:
            bool: 转矩设置是否成功
        """
        if self.current_mode != self.MODE_TORQUE:
            print(f"❌ 当前模式为 {self.get_mode_name(self.current_mode)}，需先调用 set_operation_mode(4) 切换至转矩模式")
            return False
        if self.current_control is None:
            print("❌ 未设置控制方式，需先调用 set_control_method(SYNC_CONTROL/ASYNC_CONTROL)")
            return False

        # 调用 Motor_CTL_Ser 的转矩设置方法（符合文档3.5.2节同步/异步控制逻辑）
        success = self.torque_ctl.set_torque(
            control_mode=self.current_control,
            torque=torque,
            left_torque=left_torque,
            right_torque=right_torque
        )
        if success:
            mode_desc = f"同步转矩{torque}mA" if self.current_control == 0 else f"左{left_torque}mA/右{right_torque}mA"
            print(f"✅ 转矩设置成功：{mode_desc}（单位：mA，范围±30000）")
        return success

    def set_torque_params(self, left_slope=300, right_slope=300):
        """
        设置转矩模式参数（转矩斜率，文档3.5.1节参数定义）
        Args:
            left_slope: 左电机转矩斜率（mA/s，0-32767，默认300mA/s）
            right_slope: 右电机转矩斜率（mA/s，0-32767，默认300mA/s）
        Returns:
            bool: 参数设置是否成功
        """
        # 构建SDO参数设置序列（文档3.5.2节例程：0x6087=转矩斜率）
        params_seq = [
            self._build_sdo_write(0x6087, 0x01, left_slope, 4),  # 左电机转矩斜率
            self._build_sdo_write(0x6087, 0x02, right_slope, 4)   # 右电机转矩斜率
        ]
        self.add_sequence("转矩模式参数设置", params_seq)
        success = self.send_sequence(len(self.message_sequences) - 1)
        if success:
            print(f"✅ 转矩模式参数设置成功：左右电机转矩斜率均为{left_slope}mA/s（单位：mA/s）")
        else:
            print("❌ 转矩模式参数设置失败（检查0x6087索引是否支持）")
        return success

    # -------------------------- 反馈数据读取（适配文档4章对象字典） --------------------------
    def read_feedback(self, feedback_type):
        """
        读取电机反馈数据（位置/速度/转矩，文档4章反馈索引定义）
        Args:
            feedback_type: 反馈类型（"position"/"velocity"/"torque"）
        Returns:
            dict: 反馈数据（左电机/右电机，键为"left_xxx"/"right_xxx"）
        """
        feedback = {}
        if feedback_type == "position":
            # 读取实际位置（0x6064，文档4章：子索引01=左/02=右）
            left_pos, _ = self._send_and_receive_sdo(0x6064, 0x01) if hasattr(self, '_send_and_receive_sdo') else (None, None)
            right_pos, _ = self._send_and_receive_sdo(0x6064, 0x02) if hasattr(self, '_send_and_receive_sdo') else (None, None)
            feedback["left_position"] = left_pos if left_pos is not None else 0
            feedback["right_position"] = right_pos if right_pos is not None else 0
            print(f"📌 位置反馈：左电机 {feedback['left_position']} counts | 右电机 {feedback['right_position']} counts")
        elif feedback_type == "velocity":
            # 读取实际速度（0x606C，文档4章：子索引01=左/02=右，单位0.1r/min）
            left_vel, _ = self._send_and_receive_sdo(0x606C, 0x01) if hasattr(self, '_send_and_receive_sdo') else (None, None)
            right_vel, _ = self._send_and_receive_sdo(0x606C, 0x02) if hasattr(self, '_send_and_receive_sdo') else (None, None)
            feedback["left_velocity"] = left_vel if left_vel is not None else 0
            feedback["right_velocity"] = right_vel if right_vel is not None else 0
            print(f"📌 速度反馈：左电机 {feedback['left_velocity']*0.1:.1f} r/min | 右电机 {feedback['right_velocity']*0.1:.1f} r/min")
        elif feedback_type == "torque":
            # 读取实际转矩（0x6077，文档4章：子索引01=左/02=右，单位0.1A）
            left_torque, _ = self._send_and_receive_sdo(0x6077, 0x01) if hasattr(self, '_send_and_receive_sdo') else (None, None)
            right_torque, _ = self._send_and_receive_sdo(0x6077, 0x02) if hasattr(self, '_send_and_receive_sdo') else (None, None)
            feedback["left_torque"] = left_torque if left_torque is not None else 0
            feedback["right_torque"] = right_torque if right_torque is not None else 0
            print(f"📌 转矩反馈：左电机 {feedback['left_torque']*0.1:.1f} A | 右电机 {feedback['right_torque']*0.1:.1f} A")
        else:
            print(f"❌ 无效反馈类型 {feedback_type}，仅支持：position/velocity/torque")
            return None
        return feedback

    # -------------------------- 位置监控和状态监控功能 --------------------------
    def monitor_velocity_status(self, timeout_ms=5000, background=False):
        """
        监控速度模式状态字（6041h）
        检测bit10（设置速度是否生效）和bit14（电机是否运行中）
        当两位均为1时输出"电机正常运行，继续任务"
        
        Args:
            timeout_ms: 超时时间（毫秒）
            background: 是否在后台线程中运行监控（默认False，同步运行）
            
        Returns:
            bool: 是否检测到正常运行状态（如果background=True，则立即返回True）
        """
        if background:
            # 后台线程模式：启动监控线程并立即返回
            return self._start_velocity_monitoring(timeout_ms)
        else:
            # 同步模式：在当前线程中运行监控
            print("🔍 开始监控速度模式状态...")
            start_time = time.time()
            
            while (time.time() - start_time) * 1000 < timeout_ms:
                try:
                    # 读取状态字（0x6041）
                    status_word, _ = self.read_status_word()
                    if status_word is None:
                        print("⚠️  无法读取状态字")
                        time.sleep(0.1)
                        continue
                    
                    # 解析状态字bit10（速度设置生效位）和bit14（电机运行中位）
                    bit10 = (status_word >> 10) & 0x01  # 速度设置生效
                    bit14 = (status_word >> 14) & 0x01  # 电机运行中
                    
                    print(f"📊 状态字: 0x{status_word:04X}, bit10(速度生效): {bit10}, bit14(运行中): {bit14}")
                    
                    # 检查两位是否均为1
                    if bit10 == 1 and bit14 == 1:
                        print("✅ 电机正常运行，继续任务")
                        return True
                    
                    # 等待一段时间再检查
                    time.sleep(0.1)
                    
                except Exception as e:
                    print(f"❌ 速度状态监控过程中出现错误: {e}")
                    time.sleep(0.5)
            
            print("⚠️  速度状态监控超时，未检测到正常运行状态")
            return False
    
    def _start_velocity_monitoring(self, timeout_ms=5000):
        """
        启动速度监控线程
        
        Args:
            timeout_ms: 超时时间（毫秒）
            
        Returns:
            bool: 线程启动是否成功
        """
        if self.velocity_monitor_thread is not None and self.velocity_monitor_thread.is_alive():
            print("⚠️  速度监控线程已在运行中")
            return True
        
        # 启动监控线程
        self.velocity_monitoring = True
        self.velocity_monitor_timeout = timeout_ms
        self.velocity_monitor_thread = threading.Thread(target=self._velocity_monitor_thread, daemon=True)
        self.velocity_monitor_thread.start()
        print("✅ 速度监控线程已启动（后台运行）")
        return True
    
    def _velocity_monitor_thread(self):
        """速度监控线程（检查速度模式状态）"""
        print("🔍 速度监控线程启动")
        start_time = time.time()
        
        while self.velocity_monitoring:
            try:
                # 读取状态字（0x6041）
                status_word, _ = self.read_status_word()
                if status_word is None:
                    print("⚠️  无法读取状态字")
                    time.sleep(0.1)
                    continue
                
                # 解析状态字bit10（速度设置生效位）和bit14（电机运行中位）
                bit10 = (status_word >> 10) & 0x01  # 速度设置生效
                bit14 = (status_word >> 14) & 0x01  # 电机运行中
                
                print(f"📊 状态字: 0x{status_word:04X}, bit10(速度生效): {bit10}, bit14(运行中): {bit14}")
                
                # 检查两位是否均为1
                if bit10 == 1 and bit14 == 1:
                    print("✅ 电机正常运行，继续任务")
                    self.velocity_monitoring = False
                    break
                
                # 超时检查
                if (time.time() - start_time) * 1000 >= self.velocity_monitor_timeout:
                    print("⚠️  速度状态监控超时，未检测到正常运行状态")
                    self.velocity_monitoring = False
                    break
                
                # 等待一段时间再检查
                time.sleep(0.1)
                
            except Exception as e:
                print(f"❌ 速度状态监控过程中出现错误: {e}")
                time.sleep(0.5)
        
        print("🔍 速度监控线程结束")
    
    def stop_velocity_monitoring(self):
        """停止速度监控线程"""
        if self.velocity_monitoring:
            self.velocity_monitoring = False
            if self.velocity_monitor_thread is not None:
                self.velocity_monitor_thread.join(timeout=1.0)
                if self.velocity_monitor_thread.is_alive():
                    print("⚠️  速度监控线程未能正常结束")
                else:
                    print("✅ 速度监控线程已停止")
            self.velocity_monitor_thread = None

    def _clear_quick_stop_state(self):
        """
        解除急停状态（适配CiA402状态机），增强重试机制和状态验证
        
        Returns:
            bool: 清除急停是否成功
        """
        try:
            print("   执行解除急停流程...")
            max_retries = 3
            retry_count = 0
            
            # 多次尝试解除急停
            while retry_count < max_retries:
                # 发送Quick Stop Reset命令（控制字0x80）
                cmd = self._build_sdo_write(
                    index=0x6040,
                    subindex=0x00,
                    data=0x80,  # Quick Stop Reset控制字
                    data_size=2
                )
                
                if not self.pos_ctl._send_can_data(self.send_id, cmd):
                    print(f"❌ 第{retry_count+1}次发送Quick Stop Reset命令失败")
                    retry_count += 1
                    time.sleep(0.2)
                    continue
                
                time.sleep(0.2)  # 增加等待时间，确保状态更新
                
                # 检查急停状态是否已解除
                status_word, current_state = self.read_status_word()
                if current_state != self.STATE_QUICK_STOP_ACTIVE:
                    print(f"✅ 急停状态已解除，当前状态: {self.get_state_name(current_state)}")
                    break
                
                print(f"⚠️  第{retry_count+1}次尝试后仍处于急停状态，重试...")
                retry_count += 1
                time.sleep(0.2)
            
            if retry_count >= max_retries:
                print("❌ 多次尝试解除急停失败，尝试备用方案")
                # 备用方案：直接使用Enable操作序列
                return self.send_cia402_sequence("enable")
            
            # 恢复到Operation Enabled状态
            return self.send_cia402_sequence("enable")
            
        except Exception as e:
            print(f"❌ 解除急停过程出错: {e}")
            return False

    def _start_position_monitoring(self):
        """
        启动位置监控线程
        """
        if self.position_monitor_thread is not None and self.position_monitor_thread.is_alive():
            print("⚠️  位置监控线程已在运行中")
            return
        
        # 启动监控线程
        self.position_monitoring = True
        self.position_monitor_thread = threading.Thread(target=self._position_monitor_thread, daemon=True)
        self.position_monitor_thread.start()
        print("✅ 位置监控线程已启动")

    def _position_monitor_thread(self):
        """位置监控线程（检查目标位置到达状态）"""
        print("📌 位置监控线程启动")
        start_time = time.time()
        
        # 等待位置控制激活（bit14=1）
        activation_timeout = 5.0  # 5秒激活超时
        position_activated = False
        
        while self.position_monitoring:
            try:
                # 读取状态字检查目标到达位（bit10）和位置控制激活位（bit14）
                status_word, _ = self.read_status_word()
                if status_word is None:
                    time.sleep(0.1)
                    continue
                
                # 解析状态字（bit10: 目标到达, bit14: 位置控制激活）
                bit10 = (status_word >> 10) & 0x01  # 目标到达位
                bit14 = (status_word >> 14) & 0x01  # 位置控制激活位
                
                print(f"📊 位置监控 - 状态字: 0x{status_word:04X}, bit10(到达): {bit10}, bit14(激活): {bit14}")
                
                # 首先等待位置控制激活
                if not position_activated:
                    if bit14 == 1:
                        print("✅ 位置控制已激活，开始监控目标到达")
                        position_activated = True
                        start_time = time.time()  # 重置超时计时器
                    elif time.time() - start_time > activation_timeout:
                        print("⚠️  位置控制激活超时，尝试重新激活位置控制")
                        # 使用重构后的set_position函数重新激活位置控制
                        if self.target_positions:
                            success = self.set_position(
                                position_mode=self.target_positions.get('mode', 0),
                                left_pos=self.target_positions.get('left', 0),
                                right_pos=self.target_positions.get('right', 0),
                                auto_reactivate=True
                            )
                            if success:
                                print("✅ 位置控制重新激活成功")
                                start_time = time.time()  # 重置超时计时器
                            else:
                                print("❌ 位置控制重新激活失败")
                                self.position_monitoring = False
                                break
                        else:
                            print("❌ 无法重新激活位置控制：目标位置信息缺失")
                            self.position_monitoring = False
                            break
                    time.sleep(0.1)
                    continue
                
                # 位置控制已激活，检查目标到达
                # 修改：只要目标到达位为1，就认为已到达目标位置，不再严格要求bit14必须为1
                if bit10 == 1:
                    print("✅ 检测到目标位置到达")
                    self.position_monitoring = False
                    break
                
                # 超时检查（15秒）
                if time.time() - start_time > 15:
                    print("⚠️  位置监控超时")
                    self.position_monitoring = False
                    break
                
                time.sleep(0.1)
                
            except Exception as e:
                print(f"❌ 位置监控线程错误: {e}")
                time.sleep(0.5)
        
        print("📌 位置监控线程结束")
